
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include "contract.h"
//#include "mbedtls/des.h"
#include "Utility.h"

#define INVALID_CODE		0x27

//void des3_enc_cbc(uint8_t * plain, uint8_t * cipher, uint32_t len, const uint8_t keys[16])
//{
//	mbedtls_des3_context ctx3;
//	unsigned char buf[8];
//	uint32_t i;

//	mbedtls_des3_init(&ctx3);
//	mbedtls_des3_set2key_enc(&ctx3, keys);

//	for (i = 0; i < len; i += 8) {
//		memset(buf, 0, 8);
//		memcpy(buf, plain + i, (len - i) < 8 ? (len - i): 8);
//		mbedtls_des3_crypt_ecb(&ctx3, buf, cipher + i);
//	}
//	
//	mbedtls_des3_free(&ctx3);
//}

//void des3_dec_cbc(uint8_t * cipher, uint8_t * plain, uint32_t len, const uint8_t keys[16])
//{
//	mbedtls_des3_context ctx3;
//	uint32_t i;

//	mbedtls_des3_init(&ctx3);
//	mbedtls_des3_set2key_dec(&ctx3, keys);

//	for (i = 0; i < len; i += 8) {
//		mbedtls_des3_crypt_ecb(&ctx3, cipher + i, plain + i);
//	}
//	
//	mbedtls_des3_free(&ctx3);
//}

//void des_dec(uint8_t * cipher, uint8_t * plain, uint32_t len, const uint8_t keys[8])
//{
//	mbedtls_des_context ctx;
//	uint32_t i;

//	mbedtls_des_init(&ctx);
//	mbedtls_des_setkey_dec(&ctx, keys);

//	for (i = 0; i < len; i += 8) {
//		mbedtls_des_crypt_ecb(&ctx, cipher + i, plain + i);
//	}
//	
//	mbedtls_des_free(&ctx);
//}

uint16_t swap16(uint16_t value)
{
	return SWAP16(value);
}

uint32_t swap32(uint32_t value)
{
	return SWAP32(value);
}

int hexDataToStrData(char *str, uint32_t strLen, uint8_t *hex, uint32_t hexLen, bool type)
{
	char strByte[3] = {0};
	if(str == NULL || hexLen == NULL || strLen < ((hexLen * 2)+1))return -1;
	memset(str, 0, strLen);
	for(uint32_t i=0;i<hexLen;i++)
	{
		
		snprintf(strByte, sizeof(strByte), type ? "%02X" : "%02x", hex[i]);
		strcat(str, strByte);
	}
	return 0;
}

int strDataToHexData (uint8_t *hex, char *str)
{
	uint16_t len = strlen(str);
	for(uint16_t i=0;i<len;i+=2){
		uint32_t tmp;
		char s[3] = {0};
		memcpy(s, &str[i], 2);
		if(sscanf(&str[i], "%02X", &tmp) < 0)return -1;
		hex[i/2] = tmp;
	}
	return len/2;
}

uint8_t ValueToHexCode(uint8_t val){
	if(val <= 9){
		return '0' + val;
	}
	if(0xA <= val && val <= 0xF){
		return val + ('A' - 10);
	}
	return INVALID_CODE;
}

uint8_t ValueToDecCode(uint8_t val){
	if(val <= 9){
		return '0' + val;
	}

	return INVALID_CODE;
}


uint8_t HexCharToValue(uint8_t ch){
	if('0' <= ch && ch <= '9'){
		return ch - '0';
	}
	if('a' <= ch && ch <= 'f'){
		return ch - 'a' + 10;
	}
	if('A' <= ch && ch <= 'F'){
		return ch - 'A' + 10;
	}
	return 0xFF;
}

uint8_t DecCharToValue(uint8_t ch){
	if('0' <= ch && ch <= '9'){
		return ch - '0';
	}
	return 0xFF;
}

uint8_t CharToValue(uint8_t ch, uint8_t base){
	if('0' <= ch && ch <= '9'){
		return ch - '0';
	}
	if(base == 10){
		return 0xFF;
	}
	if('a' <= ch && ch <= 'f'){
		return ch - 'a' + 10;
	}
	if('A' <= ch && ch <= 'F'){
		return ch - 'A' + 10;
	}
	return 0xFF;
}

int StrToArray(uint8_t base, const uint8_t* str, uint16_t slen, uint8_t* buf, uint16_t size){
	int i, j = 0;
	uint32_t len = slen & ((~0u) << 1);
	
	REQUIRE(base == 10 || base == 16);
	
	for(i = 0; i < len; i += 2){
		uint8_t hi = CharToValue(str[i], base);
		uint8_t lo = CharToValue(str[i+1], base);
		if(hi == 0xFF || lo == 0xFF){
			break;
		}
		if(j < size){
			buf[j++] = hi * base + lo;
		}
		else{
			break;
		}
	}
	return j;	
}

#if 0
int HexStrToBuf(uint8_t* strHex, uint32_t strLen, uint8_t* buf, uint32_t bufSize){
	int i, j = 0;
	uint32_t len = strLen & ((~0u) << 1);
	
	for(i = 0; i < len; i += 2){
		unsigned char h = HexCharToValue(buf[i]);
		unsigned char l = HexCharToValue(buf[i+1]);
		if(h == 0xFF || l == 0xFF){
			break;
		}
		if(j < bufSize){
			buf[j++] = (h << 4) | (l & 0x0F);
		}
		else{
			break;
		}
	}
	return j;	
}
#endif 


#if 0
int DecStrToBuf(uint8_t* str, uint16_t slen, uint8_t* buf, uint8_t size){
	int i, j = 0;
	uint32_t len = slen & ((~0u) << 1);
	
	for(i = 0; i < len; i += 2){
		unsigned char h = DecCharToValue(buf[i]);
		unsigned char l = DecCharToValue(buf[i+1]);
		if(h == 0xFF || l == 0xFF){
			break;
		}
		if(j < size){
			buf[j++] = (h << 4) | (l & 0x0F);
		}
		else{
			break;
		}
	}
	return j;	
}
#endif 


void DecStrToValue(uint8_t* strDec, uint8_t strLen, uint64_t* value){
	uint64_t v = 0;
	uint32_t i;
	uint8_t d;
	
	for(i = 0; i < strLen; i++){
		d = DecCharToValue(strDec[i]);
		if(d > 9){
			break;
		}
		v = v * 10 + d;
	}
	*value = v;
}

//翻转拷贝
void Mem_Reverse_Copy(uint8_t *to, const uint8_t *from, int length){
	uint8_t *t = to + length;
	int i;

	for(i = 0; i < length; i++){
		t--;
		*t = from[i];
	}
}
//翻转
void Mem_Reverse(uint8_t* buffer, int length){
	uint8_t* head = buffer;
	uint8_t* tail = buffer + length;

	if(length > 0){
		uint8_t ch;
		
		tail--;
		while(head < tail){  
			ch = *head;  
			*head++ = *tail;    /* h向尾部移动 */
			*tail-- = ch;    /* t向头部移动 */
		}  
	}	
}

//倒退拷贝
int Mem_Backward_Copy(uint8_t *to, int tsize, uint8_t *from, int length, int count){
	uint8_t *t = to + tsize;
	uint8_t *f = from + length;
	int i = 0;
	int n = UTIL_MIN(tsize, length);
	
	n = UTIL_MIN(n, count);
	
	for(i = 0; i < n; i++){
		t--;
		f--;
		*t = *f;
	}
	return n;
}


const char* StrToVal(const char* str, uint8_t base, uint64_t* val){
	const char* s = str;
	uint64_t v = (uint64_t)0;
	
	ASSERT(base == 10 || base == 16);
	
	while(*s){
		uint8_t c = CharToValue(*s, base);
		if(c == 0xFF){
			break;
		}
		v = v * base + c;
		s++;
	}
	*val = v;
	return s;
}

const char* NStrToVal(const char* str, uint16_t n, uint8_t base, uint64_t* val){
	const char* s = str;
	uint64_t v = (uint64_t)0;
	
	ASSERT(base == 10 || base == 16);
	
	while(*s && n--){
		uint8_t c = CharToValue(*s, base);
		if(c == 0xFF){
			break;
		}
		v = v * base + c;
		s++;
	}
	*val = v;
	return s;
}


//将字符串以十六进制转成U8位的数值
#if 1
unsigned char U8ToHex(char *digit, unsigned char len)
{
	unsigned char num = 0;
	while(len--)
	{
		if(*digit >= '0' && *digit <= '9')
		{
			num = num * 16 + (*digit - '0');
			digit++;
		}
		else
		{
			num = 0;
			break;
		}
	}
	return num;
}
#endif 

//将字符串以十六进制转成U32位的数值
#if 0
unsigned long U32ToHex(char *digit, unsigned char len)
{
	unsigned long num = 0;
	while(len--)
	{
		if(*digit >= '0' && *digit <= '9')
		{
			num = num * 16 + (*digit - '0');
			digit++;
		}
		else
		{
			num = 0;
			break;
		}
	}
	return num;
}
#endif 

unsigned long U32ToData(char *digit, unsigned char len)
{
	unsigned long num = 0;
	while(len--)
	{
		if(*digit >= '0' && *digit <= '9')
		{
			num = num * 10 + (*digit - '0');
			digit++;
		}
		else
		{
			num = 0;
			break;
		}
	}
	return num;
}

uint64_t BufToValue(uint8_t lsb, uint8_t buf[], uint8_t len){
	uint64_t v = 0;
	uint8_t i;
	uint8_t l = len < 8 ? len : 8;
	
	for(i = 0; i < l; i++){
		if(lsb){
			v |= (uint64_t)buf[i] << (i * 8);
		}
		else{
			v <<=8;
			v |= buf[i];
		}
	}
	return v;
}


unsigned char U8ToData(char* digit, unsigned char len){
	unsigned char num = 0;
	while(len--)
	{
		if(*digit >= '0' && *digit <= '9')
		{
			num = num * 10 + (*digit - '0');
			digit++;
		}
		else
		{
			num = 0;
			break;
		}
	}
	return num;
}

unsigned char CountSetBits(unsigned char nibble){
    static const unsigned char set_bit_num[] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};
    return set_bit_num[nibble & 0x0F];
}


#if 0
uint8_t Buffer_Xor(uint8_t buf[], uint32_t len)
{
    uint32_t i;
    uint8_t xor;

    REQUIRE(buf);

    for(xor = i = 0; i < len; i++)
    {
        xor ^= buf[i];
    }

    return xor;
}
/*Count the set bit in byte*/
uint8_t CountSetBit(uint8_t byte)
{
    static const uint8_t setbit_number_table[16] =
    /*0, 1,  2,  3,   4,  5,  6,   7,  8,  9,   A,  B,  C,  D,  E,   F*/
    {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};

    return setbit_number_table[byte & 0x0F] + setbit_number_table[byte >> 4];
}

uint16_t Odd(uint8_t data, uint8_t data_bits)
{
    uint16_t odd;
    uint8_t set_bit_count = 0;

    odd = data & (uint8_t)(0xFF >>(8 - data_bits));
    set_bit_count = CountSetBit(odd);
    if((set_bit_count & 1) == 0)
    {
        odd |= (1u << data_bits);
    }

    return odd;
}

void BitBuffer_Put(uint8_t bit_buffer[], uint32_t position, uint8_t val)
{
    uint32_t byte_offset = position / 8;
    uint32_t bit_offset = position % 8;

    if(val != 0)
    {
        bit_buffer[byte_offset] |= 1u <<  bit_offset;
    }
    else
    {
        bit_buffer[byte_offset] &= ~(1u << bit_offset);
    }
}

uint8_t BitBuffer_Get(uint8_t bit_buffer[], uint32_t bit_num)
{
    uint32_t byte_offset = bit_num / 8;
    uint32_t bit_offset = bit_num % 8;

    return (bit_buffer[byte_offset] >> bit_offset) & 0x01;
}
#endif
//consecutive zero bits
//find the number of trailing zeros in v
int Bit_ConsecutiveZeroBits(unsigned int v){
	//unsigned int v;  // find the number of trailing zeros in v
	int r;			 // put the result in r
	static const unsigned char Mod37BitPosition[] = // map a bit value mod 37 to its position
	{
	  32, 0, 1, 26, 2, 23, 27, 0, 3, 16, 24, 30, 28, 11, 0, 13, 4,
	  7, 17, 0, 25, 22, 31, 15, 29, 10, 12, 6, 0, 21, 14, 9, 5,
	  20, 8, 19, 18
	};
	r = Mod37BitPosition[(-v & v) % 37];
	return r;
}

int BitBuffer_ConsecutiveZeroBits(uint8_t bit_buffer[], uint32_t byte_count){
	int c = 0;
	int czb;
	uint32_t i;

	for(i = 0; i < byte_count; i++){
		czb = Bit_ConsecutiveZeroBits(bit_buffer[i]);
		if(czb < 8){
			c += czb;
			break;
		}
		c += 8;
	}
	return c;
}

int StrNLen(const char* string, int maxLength){
	const char* s;
	int count = maxLength;
	
	for(s = string; count && *s != '\0'; ++s, --count){}
	
	return s - string;
}


